# SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
# SPDX-License-Identifier: BSD-3-Clause
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice, this
# list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# 3. Neither the name of the copyright holder nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
# Copyright (c) 2021 ETH Zurich, Nikita Rudin

from legged_gym.envs.aliengo.aliengo_parkour_config import AliengoParkourCfg, AliengoParkourCfgPPO


class AliengoParkourCRAWLCfg( AliengoParkourCfg ):
    class env( AliengoParkourCfg.env ):
        episode_length_s = 50  # obstacle avoidance
        n_scan = 132  # 11x12
        # n_scan = 176  # 11x12(+4)
        n_priv = 3 
        n_priv_latent = 4 + 1 + 12 + 12  # 29
        n_proprio = 3 + 2 + 1 + 2 + 3 + 36 + 4 + 1 # 51 add crawl command
        # n_proprio = 3 + 2 + 3 + 36 + 4
        history_len = 10  # 570
        num_observations = n_proprio + n_scan + history_len * n_proprio + n_priv_latent + n_priv # 57 + 132 + 10*57 + 29 + 3

    class depth( AliengoParkourCfg.depth ):        
        # downside camera
        position = [0.331, 0, -0.034] 
        angle = [9, 19] 

        original = (106, 60)
        resized = (87, 58)
        horizontal_fov = 87
        buffer_len = 2
                
        scale = 1
        invert = True

        class forward_depth:
            countour_threshold = 0.
            stereo_min_distance = 0.12 # when using (240, 424) resolution
            stereo_far_distance = 2.
            stereo_far_noise_std = 0.08 # The noise std of pixels that are greater than stereo_far_noise_distance
            stereo_near_noise_std = 0.02 # The noise std of pixels that are less than stereo_far_noise_distance
            stereo_full_block_artifacts_prob = 0.008 # The probability of adding artifacts to pixels that are less than stereo_min_distance
            stereo_full_block_values = [0.0, 0.25, 0.5, 1., 3.]
            stereo_full_block_height_mean_std = [62, 1.5]
            stereo_full_block_width_mean_std = [3, 0.01]
            stereo_half_block_spark_prob = 0.02
            stereo_half_block_value = 3000 # to the maximum value directly
            sky_artifacts_prob = 0.0001
            sky_artifacts_far_distance = 2. # greater than this distance will be viewed as to the sky
            sky_artifacts_values = [0.6, 1., 1.2, 1.5, 1.8]
            sky_artifacts_height_mean_std = [2, 3.2]
            sky_artifacts_width_mean_std = [2, 3.2]
    
        class forward_camera:
            # config to simulate stero RGBD camera
            crop_top_bottom = [0, 0]
            crop_left_right = [int(60/4), int(46/4)]
            depth_range = [0.0, 1.5] # [m]

    class terrain( AliengoParkourCfg.terrain ):
        # measured_points_x = [-0.45, -0.3, -0.15, 0, 0.15, 0.3, 0.45, 0.6, 0.75, 0.9, 1.05, 1.2, 1.35, 1.5, 1.65, 1.8] # 1mx1.6m rectangle (without center line)
        # measured_points_y = [-0.75, -0.6, -0.45, -0.3, -0.15, 0., 0.15, 0.3, 0.45, 0.6, 0.75]
        loca_measured_points_x = [-0.45, -0.3, -0.15, 0., 0.15, 0.3, 0.45]
        # loca_measured_points_y = [-0.45, -0.3, -0.15, 0., 0.15, 0.3, 0.45]
        loca_measured_points_y = [-0.15, 0., 0.15]
        n_loca_points_x = 7
        n_loca_points_y = 3
        
        terrain_length = 20
        terrain_width = 4  # 5 for baseline_ex
        num_rows = 10 # number of terrain rows (levels)  # spreaded is benifitiall !
        num_cols = 35 # number of terrain cols (types)
        terrain_dict = {"parkour": 0.,
                        "parkour_hurdle": 0.1,
                        "parkour_flat": 0.,
                        "parkour_step": 0.1,
                        "parkour_gap": 0.1,
                        "my_flat_obs": 0.1,
                        "my_slop": 0.,
                        "my_stairs": 0.,
                        "virtual_obs_flat": 0.0,
                        "half_virtual_obs_flat": 0.0,}
        terrain_proportions = list(terrain_dict.values())
        virtual = True
        # obs
        platform_width = [3.5, 3]
        # ori
        y_range = [-0.4, 0.4]
        x_range = [1.5, 2.4]
        
        flat_y_range = [-1.5, 1.5]
        flat_x_range = [1.5 ,2.5]   
        stairs_y_range = [-0.5, 0.5]
        hurdle_x_range = [1.5 ,2.5] 
        side_slop_len = [0.9, 1.]
        step_len = [0.8, 2.]  
        gap_x_range = [1.2, 2.] 
        # terrain_proportions = list(terrain_dict.values())

        # trimesh only:
        slope_treshold = 1.5  # slopes above this threshold will be corrected to vertical surfaces
        origin_zero_z = True
        num_goals = 6 # 6
        
    class rewards( AliengoParkourCfg.rewards ):
        crawl_target_base_height = 0.27
        stand_target_base_height = 0.38
        resampling_time = 2.
        class scales:
            # normal rewards
            tracking_goal_vel = 1.5 * 2
            tracking_yaw = 1
            forward = 1
            
            stay = 5.
            
            stall = -1.
            base_collision = -1
            lin_vel_z = -1.0
            ang_vel_xy = -0.05  
            orientation = -1.5
            dof_acc = -2.5e-7
            num_collision = -10. 
            action_rate = -0.1 * 2.2
            delta_torques = -1.0e-7 * 2.2
            torques = -0.00001 * 2.2
            hip_pos = -0.5 
            feet_edge = -1 
            dof_error = -0.04 * 2 
            feet_stumble = -1.

            # HIM reward
            joint_power = -2e-5
            smoothness = -0.01
            foot_clearance = -0.01

            # base_target_penal = -2.0
            base_target = 3.5

class AliengoParkourCrawlCfgPPO(AliengoParkourCfgPPO):
    class estimator(AliengoParkourCfgPPO.estimator):
        num_prop = AliengoParkourCRAWLCfg.env.n_proprio
